/******************************************************************************
*  Nano-RK, a real-time operating system for sensor networks.
*  Copyright (C) 2007, Real-Time and Multimedia Lab, Carnegie Mellon University
*  All rights reserved.
*
*  This is the Open Source Version of Nano-RK included as part of a Dual
*  Licensing Model. If you are unsure which license to use please refer to:
*  http://www.nanork.org/nano-RK/wiki/Licensing
*
*  This program is free software: you can redistribute it and/or modify
*  it under the terms of the GNU General Public License as published by
*  the Free Software Foundation, version 2.0 of the License.
*
*  This program is distributed in the hope that it will be useful,
*  but WITHOUT ANY WARRANTY; without even the implied warranty of
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
*  GNU General Public License for more details.
*
*  You should have received a copy of the GNU General Public License
*  along with this program.  If not, see <http://www.gnu.org/licenses/>.
*
*  Contributing Authors (specific to this file):
*  Anthony Rowe
*  Zane Starr
*  Anand Eswaren
*******************************************************************************/

#include <nrk.h>
#include <nrk_task.h>
#include <nrk_defs.h>
#include <nrk_error.h>
#include <nrk_events.h>
#include <nrk_scheduler.h>
#include <include.h>
#include <ulib.h>
#include <nrk_timer.h>
#include <nrk_time.h>
#include <nrk_cfg.h>
#include <nrk_cpu.h>
#include <nrk_watchdog.h>
#include <nrk_platform_time.h>
#include <nrk_stats.h>

// 750 measure to 100uS
// 800 * .125 = 100us
#define CONTEXT_SWAP_TIME_BOUND    750

void inline _nrk_scheduler()
{	   
	int8_t task_ID;
	uint16_t next_wake;
	uint16_t start_time_stamp;

	nrk_int_disable();   // this should be removed...  Not needed

	#ifndef NRK_NO_BOUNDED_CONTEXT_SWAP
	_nrk_high_speed_timer_reset();
	start_time_stamp = _nrk_high_speed_timer_get();
	#endif
	
	_nrk_set_next_wakeup(MAX_SCHED_WAKEUP_TIME);
	// Set to huge number which will later get set to min
	next_wake = 60000;
	// Safety zone starts here....
	#ifdef NRK_WATCHDOG
	nrk_watchdog_reset();
	#endif


	#ifdef NRK_KERNEL_TEST
	if(_nrk_cpu_state && _nrk_os_timer_get()>nrk_max_sleep_wakeup_time)
		nrk_max_sleep_wakeup_time=_nrk_os_timer_get();
	#endif
	//while(_nrk_time_trigger>0)
	//{
	nrk_system_time.nano_secs += ((uint32_t)_nrk_prev_timer_val * NANOS_PER_TICK);
	nrk_system_time.nano_secs -= (nrk_system_time.nano_secs % (uint32_t)NANOS_PER_TICK);

	#ifdef NRK_STATS_TRACKER
	if(nrk_cur_task_TCB->task_ID == NRK_IDLE_TASK_ID)
	{
		if(_nrk_cpu_state == 2)
			_nrk_stats_sleep(_nrk_prev_timer_val);
		_nrk_stats_task_preempted(nrk_cur_task_TCB->task_ID, _nrk_prev_timer_val);
		// Add 0 time since the preempted call before set the correct value
		_nrk_stats_task_suspend(nrk_cur_task_TCB->task_ID, 0);
	}
	else
	{
	  if(nrk_cur_task_TCB->suspend_flag == 1)
		_nrk_stats_task_suspend(nrk_cur_task_TCB->task_ID, _nrk_prev_timer_val);
	  else
		_nrk_stats_task_preempted(nrk_cur_task_TCB->task_ID, _nrk_prev_timer_val);
	}
	#endif
	
	// Updating the system time so that there's no overflow (we think)
	while(nrk_system_time.nano_secs >= NANOS_PER_SEC)
	{
		nrk_system_time.nano_secs -= NANOS_PER_SEC;
		nrk_system_time.secs++;
		nrk_system_time.nano_secs -= (nrk_system_time.nano_secs%(uint32_t)NANOS_PER_TICK);
	}
	
	// If current task is suspended and not done ... remove from ready queue.
	if(nrk_cur_task_TCB->suspend_flag == 1 && nrk_cur_task_TCB->task_state != FINISHED)
	{	
		// RSRC = resource
		if(nrk_cur_task_TCB->event_suspend == RSRC_EVENT_SUSPENDED)  
			// If the current task is blocked by a resource???
			// Task is suspended
			nrk_cur_task_TCB->task_state = EVENT_SUSPENDED;
		else if( nrk_cur_task_TCB->event_suspend > 0 && nrk_cur_task_TCB->nw_flag == 0) // nw = next wakeup??
			nrk_cur_task_TCB->task_state = EVENT_SUSPENDED;
		else if( nrk_cur_task_TCB->event_suspend > 0 && nrk_cur_task_TCB->nw_flag == 1) 
			nrk_cur_task_TCB->task_state = SUSPENDED;
		else	
		{
			nrk_cur_task_TCB->task_state = SUSPENDED;
			nrk_cur_task_TCB->event_suspend = 0;
			nrk_cur_task_TCB->nw_flag = 0;
		}
		nrk_rem_from_readyQ(nrk_cur_task_TCB->task_ID);
        
        // Since a job is removed from the ready queue, it is safe to now reduce it's cpu_remaining by the _nrk_prev_timer_val
        // This is for CASH. We are now calculating excess budget for this task.
        if ( nrk_cur_task_TCB->cpu_remaining > _nrk_prev_timer_val )
        {
            nrk_cur_task_TCB->cpu_remaining -= _nrk_prev_timer_val;
        }
	}
	// nrk_print_readyQ();

	// Update cpu used value for ended task
	// If the task has used its reserve, suspend task
	// Don't disable IdleTask which is 0
	// Don't decrease cpu_remaining if reserve is 0 and hence disabled
	if(nrk_cur_task_TCB->cpu_reserve != 0 
		&& nrk_cur_task_TCB->task_ID != NRK_IDLE_TASK_ID 
		&& nrk_cur_task_TCB->task_state!=FINISHED)
	{
		// Ran out of time
		/* CBS */
		if ( nrk_cur_task_TCB->type != CBS_TASK )
		{
		/* END CBS */
			if(nrk_cur_task_TCB->cpu_remaining < _nrk_prev_timer_val )
			{
				#ifdef NRK_STATS_TRACKER
				_nrk_stats_add_violation(nrk_cur_task_TCB->task_ID);
				#endif
				nrk_kernel_error_add(NRK_RESERVE_ERROR,nrk_cur_task_TCB->task_ID);
				nrk_cur_task_TCB->cpu_remaining=0;
			}
			else // Decrease CPU remaining...
            {
				nrk_cur_task_TCB->cpu_remaining -= _nrk_prev_timer_val;
            }

			task_ID = nrk_cur_task_TCB->task_ID;
		
		/* CBS */
		}
		else // CBS task
		{
			// If we ran out of time during a CBS task
			if(nrk_cur_task_TCB->cpu_remaining <= _nrk_prev_timer_val )
			{
				// End the execution of the job during this period.
				// Sicne we're not done, leave cpu_remaining to be executed during the next period
				//nrk_rem_from_readyQ(task_ID);
                // Replenish budget
                
                nrk_queue * _cur_node = _head_node;
                        while ( _cur_node != NULL )
                        {
                            //printf( "[%d:%d]", _cur_node->task_ID, nrk_task_TCB[_cur_node->task_ID].task_state );
                            _cur_node = _cur_node->Next;
                        }
                        
                        printf( "\n" );
                        	for(task_ID=0; task_ID < NRK_MAX_TASKS; task_ID++)
                            {
                                //printf( "[%d:%u]", nrk_task_TCB[task_ID].task_ID, nrk_task_TCB[task_ID].next_period );
                            }
                        
                        printf( "\n[%d:%d]", nrk_cur_task_TCB->task_ID, nrk_cur_task_TCB->task_state );
                nrk_cur_task_TCB->next_wakeup = 0;
				nrk_cur_task_TCB->cpu_remaining = nrk_cur_task_TCB->cpu_remaining + nrk_cur_task_TCB->cpu_reserve - _nrk_prev_timer_val;
                //printf( "cpu remain: %d", nrk_cur_task_TCB->cpu_remaining );
                // Increase the deadline
				nrk_cur_task_TCB->next_period += nrk_cur_task_TCB->period;
                
                
                _cur_node = _head_node->Next;
                while( _cur_node != NULL )
                {
                    if ( nrk_task_TCB[_cur_node->task_ID].next_period >= nrk_cur_task_TCB->next_period )
                    {
                        _cur_node->Prev->Next = _head_node;
                        _head_node->Prev = _cur_node->Prev;
                        _cur_node->Prev = _head_node;
                        _head_node = _head_node->Next;
                        _head_node->Prev = NULL;
                        _cur_node->Prev->Next = _cur_node;
                        
                        _cur_node = _head_node;
                        while ( _cur_node != NULL )
                        {
                            //printf( "[%d:%d]", _cur_node->task_ID, nrk_task_TCB[_cur_node->task_ID].cpu_remaining );
                            _cur_node = _cur_node->Next;
                        }
                        
                        break;
                    }
                    _cur_node = _cur_node->Next;
                }
				//nrk_add_to_readyQ(task_ID);
			}
			else // Decrease CPU remaining... we're not done yet...
			{
				nrk_cur_task_TCB->cpu_remaining -= _nrk_prev_timer_val;
			}
			task_ID = nrk_cur_task_TCB->task_ID;
		}
		/* END CBS */
	}
    
    // CASH - If we see that there are tasks that are suspended that have some CPU remaining
    if ( nrk_cur_task_TCB->task_ID == NRK_IDLE_TASK_ID )
    {
        // We should check all the tasks, look at their deadlines and reduce their cpu_remaining by the amount of time that the idle task ate up.
        // This should work irrespective of CBS_TASK or any other task. If a job finishes early, we can use it's excess time. However, this implementation will support
        // only CBS_TASK types. This is due to the assumption that we are now looking at soft-real time task-servers.
        // Now, we should reduce the time slept from the one that has the earliest deadline and keep going from there
        uint8_t time_slept = _nrk_prev_timer_val;
        int8_t sorted_edf_array[NRK_MAX_TASKS];        
        memset( &sorted_edf_array, 0, sizeof( sorted_edf_array ) );
        
        // first sort the tasks with respect to their deadlines. we will have a valid assumption that their cpu_remaining < next_period
        // Also assume that task 0 will have the maximum period.
        for ( task_ID = 0; task_ID < NRK_MAX_TASKS; task_ID++ )
        {
            if ( nrk_task_TCB[task_ID].task_ID == -1 ) continue;
            // find the spot where it should go.
            int8_t i = 0;
            for ( ; i < NRK_MAX_TASKS; i++ )
            {
                if ( nrk_task_TCB[task_ID].next_period < nrk_task_TCB[sorted_edf_array[i]].next_period )
                {
                    int8_t j = NRK_MAX_TASKS-1;
                    for ( ; j > i; j-- )
                    {
                        sorted_edf_array[j] = sorted_edf_array[j-1];
                    }
                    sorted_edf_array[i] = task_ID;
                    break;
                }                
            }            
        }
        // smallest deadline is in the first spot in the array.
        
        int8_t i = 0;
        
        //printf( "Sorted:\n" );
        for ( i = 0 ; i < NRK_MAX_TASKS; i ++ )
        {
            //printf( "[%d:%u]", sorted_edf_array[i], nrk_task_TCB[sorted_edf_array[i]].next_period );
        }
        
        
        // This should go through all the tasks and subtract their cpu_remaining with the time_slept
        for ( task_ID = 0; task_ID < NRK_MAX_TASKS && time_slept > 0; task_ID++ )
        {
            if ( nrk_task_TCB[sorted_edf_array[task_ID]].task_state == SUSPENDED )
            {
                if ( nrk_task_TCB[sorted_edf_array[task_ID]].cpu_remaining >= time_slept )
                {                
                    nrk_task_TCB[sorted_edf_array[task_ID]].cpu_remaining -= time_slept;
                    break;
                }
                else
                {
                    time_slept -= nrk_task_TCB[sorted_edf_array[task_ID]].cpu_remaining;
                    nrk_task_TCB[sorted_edf_array[task_ID]].cpu_remaining = 0;
                }
            }
        }
        
        // We are done calculating how much cpu_remaining we have left
    }
    
    // CASH

	// Check I/O nrk_queues to add tasks with remaining cpu back...

	// Add eligible tasks back to the ready Queue
	// At the same time find the next earliest wakeup
	
	// Loop through all tasks in the system
	for(task_ID=0; task_ID < NRK_MAX_TASKS; task_ID++)
	{
		// Uninitialized TCBs -- ignore them!
		if(nrk_task_TCB[task_ID].task_ID == -1)
			continue;
		
		// Unsuspend all tasks in the system.  why???
		nrk_task_TCB[task_ID].suspend_flag = 0;
		
		// Unfinished tasks
		if( nrk_task_TCB[task_ID].task_ID != NRK_IDLE_TASK_ID 
			&& nrk_task_TCB[task_ID].task_state != FINISHED )
		{
			if(  nrk_task_TCB[task_ID].next_wakeup >= _nrk_prev_timer_val )
				nrk_task_TCB[task_ID].next_wakeup -= _nrk_prev_timer_val;
			else 
				nrk_task_TCB[task_ID].next_wakeup = 0;
		
			// Do next period book keeping.
			// next_period needs to be set such that the period is kept consistent even if other
			// wait until functions are called.
			if( nrk_task_TCB[task_ID].next_period >= _nrk_prev_timer_val )
			{
				nrk_task_TCB[task_ID].next_period -= _nrk_prev_timer_val;
			}
			else
			{
				if(nrk_task_TCB[task_ID].period > _nrk_prev_timer_val)
					nrk_task_TCB[task_ID].next_period = nrk_task_TCB[task_ID].period-_nrk_prev_timer_val;
				else
					nrk_task_TCB[task_ID].next_period = _nrk_prev_timer_val % nrk_task_TCB[task_ID].period;
			}
			if(nrk_task_TCB[task_ID].next_period == 0)
				nrk_task_TCB[task_ID].next_period = nrk_task_TCB[task_ID].period;
					
		}

        //if ( nrk_cur_task_TCB->cpu_remaining > 

		// Look for Next Task that Might Wakeup to interrupt current task
		if (nrk_task_TCB[task_ID].task_state == SUSPENDED ) {
			// printf( "Task: %d nw: %d\n",task_ID,nrk_task_TCB[task_ID].next_wakeup);
			
			// If a task needs to become READY, make it ready
			if (nrk_task_TCB[task_ID].next_wakeup == 0) {
				// printf( "Adding back %d\n",task_ID );
				
				if(nrk_task_TCB[task_ID].event_suspend > 0 && nrk_task_TCB[task_ID].nw_flag == 1)
					nrk_task_TCB[task_ID].active_signal_mask = SIG(nrk_wakeup_signal);
					
				nrk_task_TCB[task_ID].event_suspend = 0;
				nrk_task_TCB[task_ID].nw_flag = 0;
				nrk_task_TCB[task_ID].suspend_flag = 0;
				
				if(nrk_task_TCB[task_ID].num_periods == 1) 
				{
					nrk_task_TCB[task_ID].task_state = READY;
					nrk_task_TCB[task_ID].next_wakeup = nrk_task_TCB[task_ID].next_period;
					nrk_task_TCB[task_ID].cpu_remaining = nrk_task_TCB[task_ID].cpu_reserve;
                    
                    if ( nrk_task_TCB[task_ID].type == CBS_TASK )
                    {
                        int8_t i = 0;
                        for ( ; i < NRK_MAX_TASKS; i++ )
                        {
                            // If a task is suspended, then we can safely take its remaining cpu time. Why is it safe?
                            // Because priorly whenever we slept, we reduced the cpu_remaining by the right amount. So now we are allowed to reclaim it.
                            // Why are we sure that it will not clash with their deadlines?
                            // It wont because we had already reserved that much time for them beforehand and we are guaranteed that the sleep task will be
                            // scheduled if no other task is running. This is the beauty of NRK.
                            if ( nrk_task_TCB[i].task_state == SUSPENDED && nrk_task_TCB[task_ID].next_period > nrk_task_TCB[i].next_period )
                            {
                                nrk_task_TCB[task_ID].cpu_remaining += nrk_task_TCB[i].cpu_remaining;
                                nrk_task_TCB[i].cpu_remaining = 0;
                            }
                        }
                    }
                    // With this implementation we dont have a "CBS" server anymore. Since the bandwidth has now been increased by an amount that is residual from the other tasks.
                    // But this is okay, since this is residual and CASH forces us to do this.
                    // In fact this implementation might be a slight deviation from the CASH stated in the text. This is much more optimal for NRK and works properly to our knowledge.
                    
					nrk_add_to_readyQ(task_ID);				
				}
				else 
				{
					nrk_task_TCB[task_ID].cpu_remaining = nrk_task_TCB[task_ID].cpu_reserve;
					//nrk_task_TCB[task_ID].next_wakeup = nrk_task_TCB[task_ID].next_period;
					//nrk_task_TCB[task_ID].num_periods--;
					nrk_task_TCB[task_ID].next_wakeup = (nrk_task_TCB[task_ID].period*(nrk_task_TCB[task_ID].num_periods-1));
					nrk_task_TCB[task_ID].next_period = (nrk_task_TCB[task_ID].period*(nrk_task_TCB[task_ID].num_periods-1));
					nrk_task_TCB[task_ID].num_periods=1;
		//			printf( "np = %d\r\n",nrk_task_TCB[task_ID].next_wakeup);
				}
			}

			if(nrk_task_TCB[task_ID].next_wakeup != 0 && 
					nrk_task_TCB[task_ID].next_wakeup < next_wake )
			{
				// Find closest next_wake task
				next_wake = nrk_task_TCB[task_ID].next_wakeup;
			}

		}		
	}

	// ***
	// At this point all the jobs that should be ready to execute are in the
	// ready queue ordered in EDF
	// ***

	#ifdef NRK_STATS_TRACKER
		_nrk_stats_task_start(nrk_cur_task_TCB->task_ID);
	#endif
	
	// Get the task info of the task that is highest priority
	task_ID = nrk_get_high_ready_task_ID();
	nrk_high_ready_prio = nrk_task_TCB[task_ID].task_prio; // Priority of the task to be run
	nrk_high_ready_TCB = &nrk_task_TCB[task_ID]; // TCB of the task to be run
    
	// next_wake should hold next time when a suspended task might get run
	// task_ID holds the highest priority READY task ID
	// So nrk_task_TCB[task_ID].cpu_remaining holds the READY task's end time 

	// Now we pick the next wakeup (either the end of the current task, or the possible resume
	// of a suspended task) 
	if(task_ID != NRK_IDLE_TASK_ID) 
	{
		// You are a non-Idle Task
		
		// At the end of this block, next_wake is the smallest time interval between now and:
		// - (possible preemption)
		// - end of current job
		// - scheduler wake up time
		if(nrk_task_TCB[task_ID].cpu_reserve != 0 && nrk_task_TCB[task_ID].cpu_remaining < MAX_SCHED_WAKEUP_TIME)
		{
			if(next_wake > nrk_task_TCB[task_ID].cpu_remaining)
				next_wake = nrk_task_TCB[task_ID].cpu_remaining;
		}
		else 
		{ 
			if(next_wake > MAX_SCHED_WAKEUP_TIME) 
				next_wake = MAX_SCHED_WAKEUP_TIME; 
		}
	} 
	else {
		// This is the idle task
		// Make sure you wake up from the idle task a little earlier
		// if you would go into deep sleep...
		// After waking from deep sleep, the next context swap must be at least
  		// NRK_SLEEP_WAKEUP_TIME-1 away to make sure the CPU wakes up in time. 
			
		if(next_wake>NRK_SLEEP_WAKEUP_TIME) 
		{
			if(next_wake-NRK_SLEEP_WAKEUP_TIME<MAX_SCHED_WAKEUP_TIME)
			{
				if(next_wake-NRK_SLEEP_WAKEUP_TIME<NRK_SLEEP_WAKEUP_TIME){
					next_wake=NRK_SLEEP_WAKEUP_TIME-1;
				}
				else {
					next_wake=next_wake-NRK_SLEEP_WAKEUP_TIME;
				}
			} else if(next_wake>NRK_SLEEP_WAKEUP_TIME+MAX_SCHED_WAKEUP_TIME){ 
				next_wake=MAX_SCHED_WAKEUP_TIME;
			} else {
				next_wake=MAX_SCHED_WAKEUP_TIME-NRK_SLEEP_WAKEUP_TIME;
			}
		} 
	}

	/*
	// Some code to catch the case when the scheduler wakes up
	// from deep sleep and has to execute again before NRK_SLEEP_WAKEUP_TIME-1
	if(_nrk_cpu_state==2 && next_wake<NRK_SLEEP_WAKEUP_TIME-1)
	{
	nrk_int_disable();
	while(1)
		{
		nrk_spin_wait_us(60000);
		nrk_led_toggle(RED_LED);
		nrk_spin_wait_us(60000);
		nrk_led_toggle(GREEN_LED);
		printf( "crash: %d %d %d\r\n",task_ID,next_wake,_nrk_cpu_state);
		}
	}*/

	//printf( "nw = %d %d %d\r\n",task_ID,_nrk_cpu_state,next_wake);
	
	// Setting new current task priority and information from the ready queue
	nrk_cur_task_prio = nrk_high_ready_prio;
	nrk_cur_task_TCB  = nrk_high_ready_TCB;

	#ifdef NRK_KERNEL_TEST
	if(nrk_high_ready_TCB==NULL)
	{
		nrk_kprintf( PSTR( "KERNEL TEST: BAD TCB!\r\n" ));
	}
	#endif
	
	// Setup the next wake time for the scheduler (?)
	_nrk_prev_timer_val = next_wake;

	// Probably should not get into here...
	if(_nrk_os_timer_get() >= next_wake)  // just bigger then, or equal? 
	{
		// FIXME: Terrible Terrible...
		// Need to find out why this is happening...

		#ifdef NRK_KERNEL_TEST
		// Ignore if you are the idle task coming from deep sleep
		if(!(task_ID == NRK_IDLE_TASK_ID && _nrk_cpu_state == 2))
			nrk_kernel_error_add(NRK_WAKEUP_MISSED, task_ID);
		#endif

		// This is bad news, but keeps things running
		// +2 just in case we are on the edge of the last tick
		next_wake = _nrk_os_timer_get() + 2;
		_nrk_prev_timer_val = next_wake;
	} 

	//
	if(task_ID != NRK_IDLE_TASK_ID)
		_nrk_cpu_state = 0;
	
	_nrk_set_next_wakeup(next_wake);
	
	#ifndef NRK_NO_BOUNDED_CONTEXT_SWAP
	// Bound Context Swap to 100us 
	nrk_high_speed_timer_wait(start_time_stamp,CONTEXT_SWAP_TIME_BOUND);
	#endif
	
	nrk_stack_pointer_restore();
	//nrk_int_enable();
	
	nrk_start_high_ready_task();

}

